﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace SurfaceAirport
{
    class Airplane
    {
        private Vector2 location;
        private Vector2 goal;
        private float rotation;
        private float velocity = 1f;
        private Queue<Vector2> route;
        private Microsoft.Xna.Framework.Graphics.Color color = Microsoft.Xna.Framework.Graphics.Color.White;

        Random r = new Random();

        public Vector2 Location
        {
            get { return location; }
            set { location = value; }
        }
        public Microsoft.Xna.Framework.Graphics.Color Color
        {
            get { return color; }
            set { color = value; }
        }
        public float Rotation
        {
            get { return rotation; }
            set { rotation = value; }
        }
        public float Velocity
        {
            get { return velocity; }
            set { velocity = value; }
        }
        public Queue<Vector2> Route
        {
            get { return route; }
            set { route = value; }
        }
        public Vector2 Goal
        {
            get { return goal; }
            set { goal = value; }
        }

        public void AppendRoute(Vector2 dest)
        {
            if ((route.Count == 0) || (Vector2.Distance(dest, route.Last()) > 6f)) //only adds new goals if finger is moving
            {
                route.Enqueue(dest);
            }
        }

        //right now there is a one frame delay in movement when changing goals
        public void SetNextLocation()
        {
            if (goal.Equals(Vector2.Zero))
            {
                if (route.Count == 0)
                {
                    Move();
                }
                else
                {
                    goal = route.Dequeue();
                    RotateTowardsGoal();
                    
                }
            }
            else if (IsPlaneOnTarget())
            {
                if (route.Count != 0)
                {
                    //to set the rotation right
                    goal = route.Dequeue();
                    RotateTowardsGoal();
                    
                }
                else
                {
                    goal = Vector2.Zero;
                }
            }
            else //Have goal, not there yet
            {
                if (r.Next(100) <= 10) //redirect and refine in 10% of the frames
                {
                    RotateTowardsGoal();
                }
                Move();
            }
        }

        //Moves an airplane along its rotation
        private void Move()
        {
            float x = location.X + (float)Math.Sin(rotation) * velocity;
            float y = location.Y - (float)Math.Cos(rotation) * velocity; //- bc top is zero
            location = new Vector2(x, y);
        }

        private void RotateTowardsGoal()
        {
            float deltaX = goal.X - location.X;
            float deltaY = goal.Y - location.Y;
            //float hyppo = (float)Math.Sqrt(Math.Pow(deltaX,2) + Math.Pow(deltaY,2));
            //rotation = (float)Math.Acos(deltaY / hyppo);
            //rotation = (float)Math.Asin(deltaX / hyppo);
            rotation = (float)Math.Atan(deltaX / deltaY);
            if (deltaY > 0) //ugly as hell but it works and im too tired to find a real solution...
            {
                rotation = (float)Math.PI - rotation;
            }
            else  
            {
                rotation = -rotation;
            }
        }

        private bool IsPlaneOnTarget()
        {
            if(goal.Equals(Vector2.Zero)){
                return false;
            }
            //float tmp = Vector2.Distance(goal, location);
            //Console.WriteLine(tmp);
            if (Vector2.Distance(goal, location) <= 1f)
            {
                return true;
            }
            return false;
        }



        public Airplane(Vector2 location, float rotation)
        {
            this.location = location;
            this.rotation = rotation;
            route = new Queue<Vector2>();
            goal = Vector2.Zero;
        }

        public Airplane(Vector2 location, float rotation, float velocity)
        {
            this.location = location;
            this.rotation = rotation;
            this.velocity = velocity;
            Color = new Microsoft.Xna.Framework.Graphics.Color(new Vector3(velocity));
            route = new Queue<Vector2>();
            goal = Vector2.Zero;
        }
    }
}
